/**
 ******************************************************************************
 * Copyright (c) 2022 - ~, SCUT-RobotLab Development Team
 * @file    ControlFSM.h
 * @author
 * @brief
 ******************************************************************************
 */
#ifndef CONTROLFSM_H
#define CONTROLFSM_H

/* Includes ------------------------------------------------------------------*/
#include "ControlFSMData.h"
#include "FSM_State.h"
#include "Safety_Checker.h"
#include "FSM_State_Passive.h"
#include "FSM_State_StandUp.h"
#include "FSM_State_JointPD.h"
#include "FSM_State_FrontJump.h"
#include "FSM_State_Locomotion.h"
#include "FSM_State_Jump.h"

#include "common/Controllers/GaitScheduler.h"
#include "common/Controllers/LegController.h"
#include "common/ControlParameters/ControlParameters.h"
#include "common/Controllers/StateEstimatorContainer.h"
#include "common/Controllers/DesiredStateCommand.h"
#include "common/ControlParameters/UserParameters.h"
#include "common/Dynamics/Quadruped.h"


/**
 *  枚举所有操作模式
 */
enum class FSM_OperatingMode
{
  NORMAL,        //正常运行状态
  TRANSITIONING, //状态切换状态
  ESTOP,         //停止状态
  EDAMP
};

/**
 *  运行状态机FSM状态列表
 */
template <typename T>
struct FSM_StatesList
{
  FSM_State<T> *invalid;                           //（1）FSM是否使用
  FSM_State_Passive<T> *passive;                   //（2）被人为操纵调试状态
  FSM_State_JointPD<T> *jointPD;                   //（3）关节PD控制器
 //FSM_State_ImpedanceControl<T> *impedanceControl; //（4）阻抗控制
  FSM_State_StandUp<T> *standUp;                   //（5）站立过程
  //FSM_State_BalanceStand<T> *balanceStand;         //（6）站立平衡
  FSM_State_Locomotion<T> *locomotion;             //（7）运动
  //FSM_State_RecoveryStand<T> *recoveryStand;       //（8）恢复站立
  //FSM_State_Vision<T> *vision;                     //（9）可视化
  //FSM_State_BackFlip<T> *backflip;                 //（10）后空翻
  FSM_State_FrontJump<T> *frontJump;               //（11）向前跳
  FSM_State_Jump<T> *jump;               //（12）跳
};

/**
 * 处理FSM状态的父类
 */
template <typename T>
class ControlFSM
{

public:
  ControlFSM(Quadruped<T> *_quadruped,
             StateEstimatorContainer<T>* _stateEstimator,
             LegController<T> *_legController,
             GaitScheduler<T> *_gaitScheduler,
             DesiredStateCommand<T> *_desiredStateCommand,
             RobotControlParameters *controlParameters,
             UserParameters *userParameters);

  //初始化状态机
  void initialize();

  // 运行FSM逻辑，处理状态转换和正常运行
  void runFSM();

  // 这将被删除并放入SafetyChecker类
  FSM_OperatingMode safetyPreCheck();

  //位置安全检测
  FSM_OperatingMode safetyPostCheck();

  // 从请求时创建的状态列表中获取下一个FSM_State
  FSM_State<T> *getNextState(FSM_StateName stateName);

  // 打印当前状态
  void printInfo(int opt);

  // 包含所有控制相关数据
  ControlFSMData<T> data;

  // FSM状态信息
  FSM_StatesList<T> statesList; //保存fsm所有状态statesList
  FSM_State<T> *currentState;   // current FSM state
  FSM_State<T> *nextState;      // next FSM state
  FSM_StateName nextStateName;  // next FSM state name

  // 检查所有输入和命令是否安全
  SafetyChecker<T> *safetyChecker;

  TransitionData<T> transitionData;

private:
  // FSM的工作模式
  FSM_OperatingMode operatingMode;

  // 控制打印频率,模拟时间N*（0.001s）
  int printNum = 10000; 

  // 跟踪自上次信息打印以来的迭代次数
  int printIter = 0; // make larger than printNum to not print

  int iter = 0;

};


#endif
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/
